/*
 * Encoder.cpp
 *
 *  Created on: 23 Mar 2021
 *  Ported from: https://github.com/simplefoc/Arduino-FOC/blob/v2.1/src/sensors/Encoder.cpp
 */

#include "Encoder.hpp"


/*
  Encoder(int encA, int encB , int cpr, int index)
  - encA, encB    - encoder A and B pins
  - cpr           - counts per rotation number (cpm=ppm*4)
  - index pin     - (optional input)
*/

Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){

  // Encoder measurement structure init
  // hardware pins
  pinA = _encA;
  pinB = _encB;
  // counter setup
  pulse_counter = 0;
  pulse_timestamp = 0;

  cpr = _ppr;
  A_active = 0;
  B_active = 0;
  I_active = 0;
  // index pin
  index_pin = _index; // its 0 if not used

  // velocity calculation variables
  prev_Th = 0;
  pulse_per_second = 0;
  prev_pulse_counter = 0;
  prev_timestamp_us = _micros();

  // extern pullup as default
  pullup = Pullup::EXTERN;
  // enable quadrature encoder by default
  quadrature = Quadrature::ON;
}

//  Encoder interrupt callback functions
// A channel
void Encoder::handleA() {
	unsigned short A = HAL_GPIO_ReadPin(EncoderAU_Pin3_GPIO_Port, EncoderAU_Pin3_Pin);

	if (A != A_active) {
		pulse_counter += (A_active == B_active) ? 1 : -1;
		pulse_timestamp = _micros();
		A_active = A;
	}
}
// B channel
void Encoder::handleB() {
	unsigned short B = HAL_GPIO_ReadPin(EncoderBV_Pin2_GPIO_Port, EncoderBV_Pin2_Pin);

	if (B != B_active) {
		pulse_counter += (A_active != B_active) ? 1 : -1;
		pulse_timestamp = _micros();
		B_active = B;
	}
}

// I channel (index)
void Encoder::handleIndex() {
	if (hasIndex()) {
		unsigned short I = HAL_GPIO_ReadPin(EncoderIW_Pin11_GPIO_Port, EncoderIW_Pin11_Pin);

		if (I && !I_active) {
			index_found = 1;
			//align encoder on each index
			long tmp = pulse_counter;
			//current counter value
			pulse_counter = round((double)pulse_counter / (double)cpr)*cpr;
			//preserve relative speed
			prev_pulse_counter += pulse_counter - tmp;
		}
		I_active = I;
	}
}

//// Index channel
//void Encoder::handleIndex() {
//  if(hasIndex()){
//    bool I = digitalRead(index_pin);
//    if(I && !I_active){
//      index_found = true;
//      // align encoder on each index
//      long tmp = pulse_counter;
//      // corrent the counter value
//      pulse_counter = round((double)pulse_counter/(double)cpr)*cpr;
//      // preserve relative speed
//      prev_pulse_counter += pulse_counter - tmp;
//    }
//    I_active = I;
//  }
//}

/*
	Shaft angle calculation
*/
float Encoder::getAngle(){
  return  _2PI * (pulse_counter) / ((float)cpr);
}
/*
  Shaft velocity calculation
  function using mixed time and frequency measurement technique
*/
float Encoder::getVelocity(){
  // timestamp
  long timestamp_us = _micros();
  // sampling time calculation
  float Ts = (timestamp_us - prev_timestamp_us) * 1e-6;
  // quick fix for strange cases (micros overflow)
  if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;

  // time from last impulse
  float Th = (timestamp_us - pulse_timestamp) * 1e-6;
  long dN = pulse_counter - prev_pulse_counter;

  // Pulse per second calculation (Eq.3.)
  // dN - impulses received
  // Ts - sampling time - time in between function calls
  // Th - time from last impulse
  // Th_1 - time form last impulse of the previous call
  // only increment if some impulses received
  float dt = Ts + prev_Th - Th;
  pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second;

  // if more than 0.05 passed in between impulses
  if ( Th > 0.1) pulse_per_second = 0;

  // velocity calculation
  float velocity = pulse_per_second / ((float)cpr) * (_2PI);

  // save variables for next pass
  prev_timestamp_us = timestamp_us;
  // save velocity calculation variables
  prev_Th = Th;
  prev_pulse_counter = pulse_counter;
  return velocity;
}

// getter for index pin
// return -1 if no index
int Encoder::needsSearch(){
  return hasIndex() && !index_found;
}

// private function used to determine if encoder has index
int Encoder::hasIndex(){
  return index_pin != 0;
}

void Encoder::setAngle(float pulse_value) {
	pulse_counter = 0;
}

// encoder initialisation of the hardware pins
// and calculation variables
void Encoder::init(){

  // counter setup
  pulse_counter = 0;
  pulse_timestamp = _micros();
  // velocity calculation variables
  prev_Th = 0;
  pulse_per_second = 0;
  prev_pulse_counter = 0;
  prev_timestamp_us = _micros();

  // initial cpr = PPR
  // change it if the mode is quadrature
  if(quadrature == Quadrature::ON) cpr = 4*cpr;

}

